/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package sistema_fuzzy;

import java.awt.Point;
import java.util.List;

/**
 *
 * @author Thay
 */
public class Sonar {

    private int distancia;
    private float angulo;
    static private int alcance = 200;
    Point obstaculoMaisProximo;

    public Sonar() {
        distancia = 0;
        angulo = 0;
    }

    public float getAngulo() {
        return angulo;
    }

    public void setAngulo(float angulo) {
        this.angulo = angulo;
    }

    public int getDistancia() {
        return distancia;
    }

    public void setDistancia(int distancia) {
        this.distancia = distancia;
    }

    public double calcularDistancia(Point robo, List<Point> obstaculos, int deslocamento) {

        int yMin = 0, yMax = 0;
        int xMin = 0, xMax = 0;

        if (!obstaculos.isEmpty()) {

            List<Point> obstaculosX = InfraVermelho.ordenaX(obstaculos);
            List<Point> obstaculosY = InfraVermelho.ordenaY(obstaculos);

            switch (deslocamento) {
                case 37:    // Seta para a ESQUERDA
                    for (int i = obstaculosX.size() - 1; i >= 0; i--) {
                        obstaculoMaisProximo = obstaculosX.get(i);
                        if(robo.x > obstaculoMaisProximo.x+50) {
                        yMin = (int) (robo.y + Math.abs((obstaculoMaisProximo.x+50) - (robo.x))
                                * Math.tan(Math.toRadians(157.5)));
                        yMax = (int) (robo.y + Math.abs((obstaculoMaisProximo.x+50) - (robo.x))
                                * Math.tan(Math.toRadians(-157.5)));

                        if (yMin <= obstaculoMaisProximo.y + 50 &&
                                yMax >= obstaculoMaisProximo.y) {
                            // Parte superior do obstaculo fora do sonar;
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x+50) - robo.x,
                                    (obstaculoMaisProximo.y + 50) - robo.y);
                            break;
                        } else if (yMin <= obstaculoMaisProximo.y &&
                                        yMax >= obstaculoMaisProximo.y + 50)
                            // Parte inferior do obstaculo fora do sonar;
                        {
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x+50) - robo.x,
                                    (obstaculoMaisProximo.y) - robo.y);
                            break;
                        } else {
                            distancia = Integer.MAX_VALUE;
                            }
                        } else distancia = Integer.MAX_VALUE;
                    }
                    break;
                case 38:    // Seta para CIMA
                    for (int i = obstaculosY.size() - 1; i >= 0; i--) {
                        obstaculoMaisProximo = obstaculosY.get(i);
                        if(robo.y > obstaculoMaisProximo.y+50) {
                        xMin = (int) (robo.x + Math.abs((obstaculoMaisProximo.y+50) - (robo.y))
                                * Math.tan(Math.toRadians(112.5)));
                        xMax = (int) (robo.x + Math.abs((obstaculoMaisProximo.y+50) - (robo.y))
                                * Math.tan(Math.toRadians(67.5)));

                        if (xMin <= obstaculoMaisProximo.x + 50 &&
                                xMax >= obstaculoMaisProximo.x) {
                            // Lado esquerdo do obstaculo fora do sonar;
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x) - robo.x,
                                    (obstaculoMaisProximo.y + 50) - robo.y);
                            break;
                        } else if (xMin <= obstaculoMaisProximo.x &&
                                        xMax >= obstaculoMaisProximo.x+50)
                            // Lado direito do obstaculo fora do sonar;
                        {
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x) - robo.x,
                                    (obstaculoMaisProximo.y+50) - robo.y);
                            break;
                        } else {
                            distancia = Integer.MAX_VALUE;
                            }
                        } else distancia = Integer.MAX_VALUE;
                    }
                    break;
                case 39:    // Seta para a DIREITA
                //    xMax = robo.x + 50 + alcance;

                    for (int i = 0; i < obstaculosX.size(); i++) {
                        obstaculoMaisProximo = obstaculosX.get(i);
                        if(robo.x < obstaculoMaisProximo.x) {
                        yMin = (int) (robo.y + Math.abs(obstaculoMaisProximo.x - (robo.x+50))
                                * Math.tan(Math.toRadians(-22.5)));
                        yMax = (int) (robo.y + Math.abs(obstaculoMaisProximo.x - (robo.x+50))
                                * Math.tan(Math.toRadians(22.5)));
                        
                        if (yMin <= obstaculoMaisProximo.y + 50 &&
                                yMax >= obstaculoMaisProximo.y) {
                            // Parte superior do obstaculo fora do sonar;
                            distancia = (int) Math.hypot(obstaculoMaisProximo.x - (robo.x + 50),
                                    (obstaculoMaisProximo.y + 50) - (robo.y + 50));
                            break;
                        } else if (yMin <= obstaculoMaisProximo.y &&
                                        yMax >= obstaculoMaisProximo.y + 50)
                            // Parte inferior do obstaculo fora do sonar;
                        {
                            distancia = (int) Math.hypot(obstaculoMaisProximo.x - (robo.x + 50),
                                    (obstaculoMaisProximo.y+50) - (robo.y + 50));
                            break;
                        } else {
                            distancia = Integer.MAX_VALUE;
                            }
                        } else distancia = Integer.MAX_VALUE;
                    }
                    break;
                case 40:    // Seta para BAIXO
                    for (int i = 0; i < obstaculosX.size(); i++) {
                        obstaculoMaisProximo = obstaculosY.get(i);
                        if(robo.y < obstaculoMaisProximo.y+50) {
                        xMin = (int) (robo.x + Math.abs((obstaculoMaisProximo.y) - (robo.y+50))
                                * Math.tan(Math.toRadians(-112.5)));
                        xMax = (int) (robo.x + Math.abs((obstaculoMaisProximo.y) - (robo.y+50))
                                * Math.tan(Math.toRadians(-67.5)));

                        if (xMin >= obstaculoMaisProximo.x + 50 &&
                                xMax <= obstaculoMaisProximo.x) {
                            // Lado esquerdo do obstaculo fora do sonar;
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x) - robo.x,
                                    (obstaculoMaisProximo.y) - (robo.y+50));
                            break;
                        } else if (xMin >= obstaculoMaisProximo.x &&
                                        xMax <= obstaculoMaisProximo.x+50)
                            // Lado direito do obstaculo fora do sonar;
                        {
                            distancia = (int) Math.hypot((obstaculoMaisProximo.x) - robo.x,
                                    (obstaculoMaisProximo.y) - (robo.y+50));
                            break;
                        } else {
                            distancia = Integer.MAX_VALUE;
                            }
                        } else distancia = Integer.MAX_VALUE;
                    }
                    break;
            }
            System.out.println(
                    "distancia (Sonar): " + distancia);

        }

//        if (Math.abs(distancia) < alcance) {
//            System.out.println("distancia: " + distancia);



        return distancia;

    }
}
